import sys
import time
import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from threading import Thread
from typing import Optional
from PySide6.QtCore import QObject, Slot, QThread, Signal
from PySide6.QtWidgets import QMainWindow, QApplication, QMessageBox
from inters.action import Turn, Move, Drive
from inters.srv import InfoShow, TargetUpdate, TargetRemove, TargetGet, CarInfoGet
from inters.msg import CarInfo
import getopt
import os


def run_as_debug() -> bool:
    """
    是否传递了参数debug
    """
    debug = False
    # 获取文件名（含后缀）
    name = os.path.basename(__file__)
    try:
        """
        options, args = getopt.getopt(args, shortopts, longopts=[])

        参数args：一般是sys.argv[1:]。过滤掉sys.argv[0]，它是执行脚本的名字，不算做命令行参数。
        参数shortopts：短格式分析串。例如："hp:i:"，h后面没有冒号，表示后面不带参数；p和i后面带有冒号，表示后面带参数。
        参数longopts：长格式分析串列表。例如：["help", "ip=", "port="]，help后面没有等号，表示后面不带参数；ip和port后面带等号，表示后面带参数。

        返回值options是以元组为元素的列表，每个元组的形式为：(选项串, 附加参数)，如：('-i', '192.168.0.1')
        返回值args是个列表，其中的元素是那些不含'-'或'--'的参数。
        """
        opts, args = getopt.getopt(sys.argv[1:], "hd:", ["help", "debug="])
        # 处理 返回值options是以元组为元素的列表。
        for opt, arg in opts:
            if opt in ("-h", "--help"):
                print(f"{name} -d <True|False>")
                print(f"or: {name} --debug=<True|False>")
                sys.exit()
            elif opt in ("-d", "--debug"):
                debug = bool(arg)
        print("debug为：", debug)

        # 打印 返回值args列表，即其中的元素是那些不含'-'或'--'的参数。
        for i in range(0, len(args)):
            print("参数 %s 为：%s" % (i + 1, args[i]))
    except getopt.GetoptError:
        print(f"Error: {name} -d <True|False>")
        print(f"   or: {name} --debug=<True|False>")
    return debug


if run_as_debug():
    from master_part.master_window_ui import Ui_MainWindow
else:
    from kitt.master_part.master_window_ui import Ui_MainWindow


class MainWindow(QMainWindow, Ui_MainWindow):
    def __init__(self):
        super(MainWindow, self).__init__()
        self.setupUi(self)

        thd_srv = Thread(target=self.start_srv)
        thd_srv.start()

        self.btn_left.mousePressEvent = self.left_mouse_press
        self.btn_left.mouseReleaseEvent = self.mouse_release
        self.btn_right.mousePressEvent = self.right_mouse_press
        self.btn_right.mouseReleaseEvent = self.mouse_release
        self.btn_forward.mousePressEvent = self.forward_mouse_press
        self.btn_forward.mouseReleaseEvent = self.mouse_release
        self.btn_backward.mousePressEvent = self.backward_mouse_press
        self.btn_backward.mouseReleaseEvent = self.mouse_release
        self.closeEvent = self.on_close
        self.btn_show_info.clicked.connect(self.show_info_clicked)
        self.btn_target_update.clicked.connect(self.target_update_clicked)
        self.btn_target_remove.clicked.connect(self.target_remove_clicked)
        self.btn_target_get.clicked.connect(self.target_get_clicked)
        self.btn_car_info_get.clicked.connect(self.car_info_get_clicked)
        self.btn_start_drive.clicked.connect(self.start_drive_clicked)

        self.win_closed = False  # 关闭窗口
        self.car_move_direction = 0  # 移动方向
        self.car_moving = False  # 移动中
        self.car_move_step = 10  # 单次移动距离
        self.car_turn_direction = 0  # 旋转方向
        self.car_turning = False  # 旋转中
        self.car_turn_step = 10  # 单次旋转角度
        self.show_info = True  # 显示车信息

        thd_keep = Thread(target=self.start_keep)
        thd_keep.start()

    def on_close(self, event):
        self.win_closed = True
        rclpy.try_shutdown()

    def start_srv(self):
        rclpy.init()
        self.master = Master("Master", self)
        rclpy.spin(self.master)
        self.master.destroy_node()
        rclpy.try_shutdown()

    def start_keep(self):
        while not self.win_closed:
            if self.car_turning and not self.master.turning:
                self.master.send_turn_goal(
                    self.car_turn_direction, float(self.car_turn_step)
                )
                pass
            if self.car_moving and not self.master.moving:
                self.master.send_move_goal(
                    self.car_move_direction, float(self.car_move_step)
                )
                pass
            time.sleep(0.01)
            pass
        pass

    def left_mouse_press(self, event):
        self.car_turn_direction = 0
        self.car_turn_step = self.spb_turn_step.value()
        self.car_turning = True
        pass

    def right_mouse_press(self, event):
        self.car_turn_direction = 1
        self.car_turn_step = self.spb_turn_step.value()
        self.car_turning = True
        pass

    def forward_mouse_press(self, event):
        self.car_move_direction = 0
        self.car_move_step = self.spb_move_step.value()
        self.car_moving = True
        pass

    def backward_mouse_press(self, event):
        self.car_move_direction = 1
        self.car_move_step = self.spb_move_step.value()
        self.car_moving = True
        pass

    def mouse_release(self, event):
        self.car_moving = False
        self.car_turning = False
        pass

    def show_angle(self, angle):
        self.lb_angle.setText(f"{angle:.2f}")
        self.lb_car_world_angle.setText(f"{angle:.2f}")

    def show_location(self, x, y):
        self.lb_location.setText(f"({x:.2f}, {y:.2f})")
        self.lb_car_world_x.setText(f"{x:.2f}")
        self.lb_car_world_y.setText(f"{y:.2f}")

    def show_target(self, has_target, x, y):
        if not has_target:
            self.le_current_target_x.setText("无")
            self.le_current_target_y.setText("无")
        else:
            self.le_current_target_x.setText(f"{x:.2f}")
            self.le_current_target_y.setText(f"{y:.2f}")

    def show_info_clicked(self, event):
        self.btn_show_info.setEnabled(False)
        self.master.send_info_show(not self.show_info)
        pass

    def change_show_info(self, show):
        if show:
            self.btn_show_info.setText("隐藏车信息")
            self.show_info = True
        else:
            self.btn_show_info.setText("显示车信息")
            self.show_info = False

        self.btn_show_info.setEnabled(True)

    def target_update_clicked(self, event):
        self.btn_target_update.setEnabled(False)
        try:
            x = float(self.le_target_x.text())
            y = float(self.le_target_y.text())
            self.master.send_target_update(x, y)
        except ValueError:
            QMessageBox.critical(
                self,
                "错误",
                "请填写数值",
                buttons=QMessageBox.Ok,
                defaultButton=QMessageBox.Ok,
            )
            self.btn_target_update.setEnabled(True)
            pass
        except Exception as err:
            QMessageBox.critical(
                self,
                "错误",
                str(err.args),
                buttons=QMessageBox.Ok,
                defaultButton=QMessageBox.Ok,
            )
            self.btn_target_update.setEnabled(True)
            pass

        pass

    def target_update(self, target_x, target_y):
        self.show_target(True, target_x, target_y)
        self.btn_target_update.setEnabled(True)
        pass

    def target_remove_clicked(self, event):
        self.btn_target_remove.setEnabled(False)
        self.master.send_target_remove()
        pass

    def target_remove(self):
        self.btn_target_remove.setEnabled(True)
        self.le_current_target_x.setText("无")
        self.le_current_target_y.setText("无")

    def target_get_clicked(self, event):
        has_target, target_x, target_y = self.master.send_target_get()
        self.show_target(has_target, target_x, target_y)
        QMessageBox.information(
            self, "提示", "获取成功", buttons=QMessageBox.Ok, defaultButton=QMessageBox.Ok
        )
        pass

    def car_info_get_clicked(self, event):
        angle, world_x, world_y = self.master.send_car_info_get()
        self.show_angle(angle)
        self.show_location(world_x, world_y)
        QMessageBox.information(
            self, "提示", "获取成功", buttons=QMessageBox.Ok, defaultButton=QMessageBox.Ok
        )
        pass

    def car_info_received(self, world_x, world_y, world_angle):
        self.show_angle(world_angle)
        self.show_location(world_x, world_y)
        pass

    def start_drive_clicked(self):
        self.lb_drive_tip.setText("自动驾驶中……")
        if self.rb_expert_system.isChecked():
            self.master.send_drive_goal(0)
        if self.rb_neural_network.isChecked():
            self.master.send_drive_goal(1)
        pass

    def drive_done(self):
        self.lb_drive_tip.setText("自动驾驶结束")
        pass


class Master(Node):
    def __init__(self, node_name, win: MainWindow):
        super().__init__(node_name=node_name)

        self.win = win
        self.turning = False
        self.moving = False
        self.driving = False
        self._turn_action_client = ActionClient(self, Turn, "/car/turn")
        self._move_action_client = ActionClient(self, Move, "/car/move")
        self._drive_action_client = ActionClient(self, Drive, "/brain/drive")
        self._info_show_client = self.create_client(InfoShow, "/car/info/show")
        self._target_update_client = self.create_client(
            TargetUpdate, "/car/target/update"
        )
        self._target_remove_client = self.create_client(
            TargetRemove, "/car/target/remove"
        )
        self._car_info_subscription = self.create_subscription(
            CarInfo, "/car/info", self.car_info_listener_callback, 10
        )
        self._car_info_subscription  # 写这行可以避免变量未使用警告
        self._car_info_get_client = self.create_client(CarInfoGet, "/car/info/get")
        self._target_get_client = self.create_client(TargetGet, "/car/target/get")

    def send_target_get(self):
        while not self._target_get_client.wait_for_service(1.0):
            self.get_logger().info("service not available, waiting again...")
            return
        req = TargetGet.Request()
        # target_get_future = self._target_get_client.call_async(req)
        # info_show_future.add_done_callback(self.info_show_response_callback)
        # rclpy.spin_until_future_complete(self, target_get_future)
        # result = target_get_future.result()
        result = self._target_get_client.call(req)
        return result.has_target, result.world_x, result.world_y

    def send_car_info_get(self):
        while not self._car_info_get_client.wait_for_service(timeout_sec=1.0):
            self.get_logger().info("service not available, waiting again...")
            return
        req = CarInfoGet.Request()
        # car_info_get_future = self._car_info_get_client.call_async(req)
        # info_show_future.add_done_callback(self.info_show_response_callback)
        # rclpy.spin_until_future_complete(self, car_info_get_future)
        # result = car_info_get_future.result()
        result = self._car_info_get_client.call(req)
        return result.world_x, result.world_y, result.world_angle

    def car_info_listener_callback(self, msg):
        self.get_logger().info(
            f"I heard car info: ({msg.world_angle:.2f}, {msg.world_x:.2f}, {msg.world_y:.2f})"
        )
        self.win.car_info_received(msg.world_x, msg.world_y, msg.world_angle)

    def send_target_remove(self):
        self._target_remove_client.wait_for_service()
        req = TargetRemove.Request()
        target_remove_future = self._target_remove_client.call_async(req)
        target_remove_future.add_done_callback(self.target_remove_response_callback)

    def target_remove_response_callback(self, future):
        result = future.result()
        self.win.target_remove()

    def send_target_update(self, x, y):
        self._target_update_client.wait_for_service()
        req = TargetUpdate.Request()
        req.x, req.y = x, y
        target_update_future = self._target_update_client.call_async(req)
        target_update_future.add_done_callback(self.target_update_response_callback)

    def target_update_response_callback(self, future):
        result = future.result()
        self.win.target_update(result.target_x, result.target_y)

    def send_info_show(self, show):
        self._info_show_client.wait_for_service()
        req = InfoShow.Request()
        req.show = show
        info_show_future = self._info_show_client.call_async(req)
        info_show_future.add_done_callback(self.info_show_response_callback)
        # rclpy.spin_until_future_complete(self, self._info_show_future)

    def info_show_response_callback(self, future):
        result = future.result()
        self.win.change_show_info(result.final_show)

    # region 转向服务

    def send_turn_goal(self, direction, angle):
        if self.turning:
            return

        self.turning = True
        goal_msg = Turn.Goal()
        goal_msg.direction = direction
        goal_msg.angle = angle
        self._turn_action_client.wait_for_server()
        turn_send_goal_future = self._turn_action_client.send_goal_async(
            goal_msg, feedback_callback=self.turn_feedback_callback
        )
        turn_send_goal_future.add_done_callback(self.turn_goal_response_callback)

    def turn_feedback_callback(self, feedback_msg):
        feedback = feedback_msg.feedback
        self.get_logger().info(
            f"Received feedback: ({feedback.angle:.2f},{feedback.world_angle:.2f})"
        )
        self.win.show_angle(feedback.world_angle)

    def turn_goal_response_callback(self, future):
        goal_handle = future.result()
        if not goal_handle.accepted:
            self.get_logger().info("Goal rejected :(")
            self.turning = False
            return

        self.get_logger().info("Goal accepted :)")
        turn_get_result_future = goal_handle.get_result_async()
        turn_get_result_future.add_done_callback(self.turn_get_result_callback)

    def turn_get_result_callback(self, future):
        result = future.result().result
        self.get_logger().info(f"Result: ({result.angle:.2f},{result.world_angle:.2f})")
        self.win.show_angle(result.world_angle)
        self.turning = False

    # endregion

    # region 移动服务

    def send_move_goal(self, direction, distance):
        if self.moving:
            return

        self.moving = True
        goal_msg = Move.Goal()
        goal_msg.direction = direction
        goal_msg.distance = distance
        self._move_action_client.wait_for_server()
        move_send_goal_future = self._move_action_client.send_goal_async(
            goal_msg, feedback_callback=self.move_feedback_callback
        )
        move_send_goal_future.add_done_callback(self.move_goal_response_callback)

    def move_feedback_callback(self, feedback_msg):
        feedback = feedback_msg.feedback
        self.get_logger().info(
            f"Received feedback: ({feedback.distance:.2f},{feedback.world_x:.2f},{feedback.world_y:.2f})"
        )
        self.win.show_location(feedback.world_x, feedback.world_y)

    def move_goal_response_callback(self, future):
        goal_handle = future.result()
        if not goal_handle.accepted:
            self.get_logger().info("Goal rejected :(")
            self.moving = False
            return

        self.get_logger().info("Goal accepted :)")
        move_get_result_future = goal_handle.get_result_async()
        move_get_result_future.add_done_callback(self.move_get_result_callback)

    def move_get_result_callback(self, future):
        result = future.result().result
        self.get_logger().info(
            f"Result: ({result.distance:.2f},{result.world_x:.2f},{result.world_y:.2f})"
        )
        self.win.show_location(result.world_x, result.world_y)
        self.moving = False

    # endregion

    # region 驾驶服务

    def send_drive_goal(self, drive_type):
        if self.driving:
            return

        self.driving = True
        goal_msg = Drive.Goal()
        goal_msg.drive_type = drive_type
        self._drive_action_client.wait_for_server()
        drive_send_goal_future = self._drive_action_client.send_goal_async(
            goal_msg, feedback_callback=self.drive_feedback_callback
        )
        drive_send_goal_future.add_done_callback(self.drive_goal_response_callback)

    def drive_feedback_callback(self, feedback_msg):
        feedback = feedback_msg.feedback
        self.get_logger().info(
            f"/brain/driver received feedback: ({feedback.angle:.2f},{feedback.distance:.2f})"
        )
        # self.win.show_location(feedback.world_x, feedback.world_y)

    def drive_goal_response_callback(self, future):
        goal_handle = future.result()
        if not goal_handle.accepted:
            self.get_logger().info("/brain/drive goal rejected :(")
            self.driving = False
            return

        self.get_logger().info("/brain/drive goal accepted :)")
        drive_get_result_future = goal_handle.get_result_async()
        drive_get_result_future.add_done_callback(self.drive_get_result_callback)

    def drive_get_result_callback(self, future):
        result = future.result().result
        self.get_logger().info(
            f"/brain/drive result: ({result.world_angle:.2f},{result.world_x:.2f},{result.world_y:.2f})"
        )
        self.win.drive_done()
        self.driving = False

    # endregion


def main(args=None):
    app = QApplication(sys.argv)
    win = MainWindow()
    win.show()
    sys.exit(app.exec())


if __name__ == "__main__":
    # sys.argv[1:]为要处理的参数列表，sys.argv[0]为脚本名，所以用sys.argv[1:]过滤掉脚本名。
    main(sys.argv[1:])
